#include <QtCore/QCoreApplication>
#include <opencv2/video/background_segm.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>

int numberOfGauss = 5; //default value  = 5
int oneoveralpha = 200; //default value  = 200, alpha = learning rate
float B = 0.8; //default value  = 0.7
float matching_deviation = 2.5; //default value = 2.5

typedef struct MixData1
{
    float match_sum;
    float weight;
    float mean;
    float variance;
}
MixData1;

typedef struct MixData3
{
    float match_sum;
    float weight;
    float mean[3];
    float variance[3];
}
MixData3;

void update8uC1(CvGaussBGModel* bg_model)
{
    int K = bg_model->params.n_gauss;
    float T = bg_model->params.bg_threshold;
    int height = bg_model->background->height;
    int width = bg_model->background->width;
    MixData1 *g_point = (MixData1 *) ((CvMat*)(bg_model->g_point))->data.ptr;
    MixData1 *mptr = g_point;

    for(int y=0; y<height; y++)
    {
        for (int x=0; x<width; x++, mptr+=K)
        {
            int pos = bg_model->background->widthStep*y + x;
            float mean = 0.0;
            float wsum = 0.0;
            int kForeground = K;

            for(int k=0; k<K; k++)
            {
                wsum += mptr[k].weight;
                if (wsum > T)
                {
                    kForeground = k+1;
                    break;
                }
            }

            for(int k=0; k<kForeground; k++)
            {
                mean += mptr[k].weight * mptr[k].mean;
            }

            bg_model->background->imageData[pos] = (uchar) (mean/wsum);
        }
    }
}

void update8uC3(CvGaussBGModel* bg_model)
{
    int K = bg_model->params.n_gauss;
    float T = bg_model->params.bg_threshold;
    int nchannels = bg_model->background->nChannels;
    int height = bg_model->background->height;
    int width = bg_model->background->width;
    MixData3 *g_point = (MixData3 *) ((CvMat*)(bg_model->g_point))->data.ptr;
    MixData3 *mptr = g_point;

    for(int y=0; y<height; y++)
    {
        for (int x=0; x<width; x++, mptr+=K)
        {
            int pos = bg_model->background->widthStep*y + x*nchannels;
            float mean[3] = {0.0, 0.0, 0.0};
            float wsum = 0.0;
            int kForeground = K;

            for(int k=0; k<K; k++)
            {
                wsum += mptr[k].weight;
                if (wsum > T)
                {
                    kForeground = k+1;
                    break;
                }
            }

            for(int k=0; k<kForeground; k++)
            {
                for(int m=0; m<nchannels; m++)
                {
                    mean[m] += mptr[k].weight * mptr[k].mean[m];
                }
            }

            for(int m=0; m<nchannels; m++)
            {
                bg_model->background->imageData[pos+m] = (uchar) (mean[m]/wsum); //+0.5
            }
        }
    }
}


void help()
{
    printf("\nDo background segmentation, especially demonstrating the use of cvUpdateBGStatModel().\n"
           "Learns the background at the start and then segments.\n"
           "Learning is togged by the space key. Will read from file or camera\n"
           "Call:\n"
           "./  bgfg_segm [file name -- if no name, read from camera]\n\n");
}

using namespace cv;

//this is a sample for foreground detection functions
int main(int argc, char** argv)
{
    IplImage*       tmp_frame = NULL;
    CvCapture*      cap = NULL;
    bool update_bg_model = true;

    if( argc < 2 )
        cap = cvCaptureFromCAM(0);
    else
        cap = cvCaptureFromFile(argv[1]);
    help();

    if( !cap )
    {
        printf("can not open camera or video file\n");
        return -1;
    }

    tmp_frame = cvQueryFrame(cap);
    if(!tmp_frame)
    {
        printf("can not read data from the video source\n");
        return -1;
    }

    //cvNamedWindow("BG", 1);
    cvNamedWindow("FG", 1);
    cvNamedWindow("GG", 1);

    CvBGStatModel* bg_model = 0;

    for( int fr = 1; tmp_frame; tmp_frame = cvQueryFrame(cap), fr++ )
    {
        if(!bg_model)
        {
            //create BG model
            bg_model = cvCreateGaussianBGModel( tmp_frame );
            ((CvGaussBGModel*)bg_model)->params.n_gauss = numberOfGauss;
            ((CvGaussBGModel*)bg_model)->params.win_size = oneoveralpha;
            ((CvGaussBGModel*)bg_model)->params.bg_threshold = B;
            ((CvGaussBGModel*)bg_model)->params.std_threshold = matching_deviation;
            continue;
        }

        //double t = (double)cvGetTickCount();
        cvUpdateBGStatModel( tmp_frame, bg_model, update_bg_model ? -1 : 0 );
        //update8uC3((CvGaussBGModel*)bg_model); //why doesnt this work!!!
        //t = (double)cvGetTickCount() - t;
        //printf( "%d. %.1f\n", fr, t/(cvGetTickFrequency()*1000.) );
        //cvShowImage("BG", bg_model->background);
        vector<vector<Point> > contours;
        vector<Vec4i> hierarchy;

        Mat image( bg_model->foreground);
        findContours(image, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
        Mat dst(tmp_frame);
//      This code paints the all foreground regions to white !!
//        int idx = 0;
//        if(hierarchy.size() > 0)
//        {
//            for( ; idx >= 0; idx = hierarchy[idx][0] )
//            {
//                Scalar color( 255, 255, 255 );
//                drawContours( dst, contours, idx, color, CV_FILLED, 8, hierarchy );
//            }
//        }

        // This code puts a rectangle around the foreground regions which hase a larger size then 'minSize'
        int minSize = 25;
        for(int i = 0; i < contours.size(); i++)
        {
            if(contours[i].size() > minSize)
            {
                Rect box = boundingRect( Mat(contours[i]) );
                rectangle(dst, box, Scalar(0, 0, 255), 3);
            }
        }

        cvShowImage("FG", bg_model->foreground);
        imshow("GG", dst);
        char k = cvWaitKey(5);
        if( k == 27 ) break;
        if( k == ' ' )
        {
            update_bg_model = !update_bg_model;
            if( update_bg_model )
                printf("Background update is on\n");
            else
                printf("Background update is off\n");
        }
    }


    cvReleaseBGStatModel( &bg_model );
    cvReleaseCapture( &cap );

    return 0;
}
